// Execute.c
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <assert.h>
#include "yemu.h"

void ExecuteDebug(struct ARCHINFO ArchInfo, struct INSTINFO InstInfo) {
    const char* RegName[32] = {
    "zero", "ra", "sp", "gp", "tp", "t0", "t1", "t2",
    "s0", "s1", "a0", "a1", "a2", "a3", "a4", "a5",
    "a6", "a7", "s2", "s3", "s4", "s5", "s6", "s7",
    "s8", "s9", "s10", "s11", "t3", "t4", "t5", "t6"
    };
    
    uint8_t rd = InstInfo.rd;
    uint8_t rs1 = InstInfo.rs1;
    uint8_t rs2 = InstInfo.rs2;
    uint64_t imm = InstInfo.imm;

    printf("Mem[%08llx] = %08x      ", ArchInfo.PC, InstInfo.Inst);

    switch (InstInfo.InstKind) 
    {
    // R-type
    case ADD:   printf("ADD   %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;
    case ADDW:  printf("ADDW  %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;
    case SUB:   printf("SUB   %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;
    case SUBW:  printf("SUBW  %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;
    case SLL:   printf("SLL   %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;
    case SLLW:  printf("SLLW  %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;
    case SRL:   printf("SRL   %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;
    case SRLW:  printf("SRLW  %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;
    case SRA:   printf("SRA   %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;
    case SRAW:  printf("SRAW  %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;
    case AND:   printf("AND   %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;
    case OR:    printf("OR    %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;
    case XOR:   printf("XOR   %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;
    case SLT:   printf("SLT   %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;
    case SLTU:  printf("SLTU  %s, %s, %s", RegName[rd], RegName[rs1], RegName[rs2]); break;

    // I-type
    case ADDI:  printf("ADDI  %s, %s, %lld", RegName[rd], RegName[rs1], (int64_t)imm); break;
    case ADDIW: printf("ADDIW %s, %s, %lld", RegName[rd], RegName[rs1], (int64_t)imm); break;
    case ANDI:  printf("ANDI  %s, %s, %lld", RegName[rd], RegName[rs1], (int64_t)imm); break;
    case ORI:   printf("ORI   %s, %s, %lld", RegName[rd], RegName[rs1], (int64_t)imm); break;
    case XORI:  printf("XORI  %s, %s, %lld", RegName[rd], RegName[rs1], (int64_t)imm); break;
    case SLTI:  printf("SLTI  %s, %s, %lld", RegName[rd], RegName[rs1], (int64_t)imm); break;
    case SLTIU: printf("SLTIU %s, %s, %lld", RegName[rd], RegName[rs1], (int64_t)imm); break;
    case SLLI:  printf("SLLI  %s, %s, %lld", RegName[rd], RegName[rs1], (int64_t)imm); break;
    case SLLIW: printf("SLLIW %s, %s, %lld", RegName[rd], RegName[rs1], (int64_t)imm); break;
    case SRLI:  printf("SRLI  %s, %s, %lld", RegName[rd], RegName[rs1], (int64_t)imm); break;
    case SRLIW: printf("SRLIW %s, %s, %lld", RegName[rd], RegName[rs1], (int64_t)imm); break;
    case SRAI:  printf("SRAI  %s, %s, %lld", RegName[rd], RegName[rs1], (int64_t)imm); break;
    case SRAIW: printf("SRAIW %s, %s, %lld", RegName[rd], RegName[rs1], (int64_t)imm); break;
    case JALR:  printf("JALR  %s, %s, %lld", RegName[rd], RegName[rs1], (int64_t)imm); break;

    // Load
    case LB:  printf("LB    %s, %lld(%s)", RegName[rd], (int64_t)imm, RegName[rs1]); break;
    case LH:  printf("LH    %s, %lld(%s)", RegName[rd], (int64_t)imm, RegName[rs1]); break;
    case LW:  printf("LW    %s, %lld(%s)", RegName[rd], (int64_t)imm, RegName[rs1]); break;
    case LD:  printf("LD    %s, %lld(%s)", RegName[rd], (int64_t)imm, RegName[rs1]); break;
    case LBU: printf("LBU   %s, %lld(%s)", RegName[rd], (int64_t)imm, RegName[rs1]); break;
    case LHU: printf("LHU   %s, %lld(%s)", RegName[rd], (int64_t)imm, RegName[rs1]); break;
    case LWU: printf("LWU   %s, %lld(%s)", RegName[rd], (int64_t)imm, RegName[rs1]); break;

    // Store
    case SB:  printf("SB    %s, %lld(%s)", RegName[rs2], (int64_t)imm, RegName[rs1]); break;
    case SH:  printf("SH    %s, %lld(%s)", RegName[rs2], (int64_t)imm, RegName[rs1]); break;
    case SW:  printf("SW    %s, %lld(%s)", RegName[rs2], (int64_t)imm, RegName[rs1]); break;
    case SD:  printf("SD    %s, %lld(%s)", RegName[rs2], (int64_t)imm, RegName[rs1]); break;

    // Branch
    case BEQ:  printf("BEQ   %s, %s, %08llx", RegName[rs1], RegName[rs2], ArchInfo.PC + imm); break;
    case BNE:  printf("BNE   %s, %s, %08llx", RegName[rs1], RegName[rs2], ArchInfo.PC + imm); break;
    case BLT:  printf("BLT   %s, %s, %08llx", RegName[rs1], RegName[rs2], ArchInfo.PC + imm); break;
    case BGE:  printf("BGE   %s, %s, %08llx", RegName[rs1], RegName[rs2], ArchInfo.PC + imm); break;
    case BLTU: printf("BLTU  %s, %s, %08llx", RegName[rs1], RegName[rs2], ArchInfo.PC + imm); break;
    case BGEU: printf("BGEU  %s, %s, %08llx", RegName[rs1], RegName[rs2], ArchInfo.PC + imm); break;

    // Jump & U-type
    case JAL:   printf("JAL   %s, %08llx", RegName[rd], ArchInfo.PC + imm); break;
    case LUI:   printf("LUI   %s, 0x%llx", RegName[rd], imm); break;
    case AUIPC: printf("AUIPC %s, 0x%llx", RegName[rd], imm); break;

    default:
        printf("Unknown or unimplemented instruction");
        assert(0);
    }
    printf("\n");
    return ;
}

struct ARCHINFO Execute(struct ARCHINFO ArchInfo, struct INSTINFO InstInfo) {
    
    // ExecuteDebug(ArchInfo, InstInfo);

    uint8_t rd = InstInfo.rd;
    uint8_t rs1 = InstInfo.rs1;
    uint8_t rs2 = InstInfo.rs2;
    uint64_t imm = InstInfo.imm;

    int trigger = 0;
    int taken = 0;

    uint64_t raddr = 0;
    uint64_t rdata0 = 0;
    uint64_t rdata1 = 0;
    uint64_t rdata2 = 0;
    uint64_t rdata3 = 0;
    uint64_t rdata4 = 0;
    uint64_t rdata5 = 0;
    uint64_t rdata6 = 0;
    uint64_t rdata7 = 0;

    uint64_t waddr = 0;
    uint8_t wdata0 = 0;
    uint8_t wdata1 = 0;
    uint8_t wdata2 = 0;
    uint8_t wdata3 = 0;
    uint8_t wdata4 = 0;
    uint8_t wdata5 = 0;
    uint8_t wdata6 = 0;
    uint8_t wdata7 = 0;

    switch (InstInfo.InstKind) {
        // Reg-Reg
        case ADD:
            ArchInfo.Regs[rd] = ArchInfo.Regs[rs1] + ArchInfo.Regs[rs2];
            break;
        case SUB:
            ArchInfo.Regs[rd] = ArchInfo.Regs[rs1] - ArchInfo.Regs[rs2];
            break;
        case AND:
            ArchInfo.Regs[rd] = ArchInfo.Regs[rs1] & ArchInfo.Regs[rs2];
            break;
        case OR:
            ArchInfo.Regs[rd] = ArchInfo.Regs[rs1] | ArchInfo.Regs[rs2];
            break;
        case XOR:
            ArchInfo.Regs[rd] = ArchInfo.Regs[rs1] ^ ArchInfo.Regs[rs2];
            break;
        case SLT:
            ArchInfo.Regs[rd] = (int64_t)ArchInfo.Regs[rs1] < (int64_t)ArchInfo.Regs[rs2];
            break;
        case SLTU:
            ArchInfo.Regs[rd] = ArchInfo.Regs[rs1] < ArchInfo.Regs[rs2];
            break;
        case SLL:
            ArchInfo.Regs[rd] = ArchInfo.Regs[rs1] << (ArchInfo.Regs[rs2] & 0x3F);
            break;
        case SRL:
            ArchInfo.Regs[rd] = ArchInfo.Regs[rs1] >> (ArchInfo.Regs[rs2] & 0x3F);
            break;
        case SRA:
            ArchInfo.Regs[rd] = (int64_t)ArchInfo.Regs[rs1] >> (ArchInfo.Regs[rs2] & 0x3F);
            break;
        case ADDW:
            ArchInfo.Regs[rd] = (int64_t)((int32_t)ArchInfo.Regs[rs1] + (int32_t)(ArchInfo.Regs[rs2]));
            break;
        case SUBW:
            ArchInfo.Regs[rd] = (int64_t)((int32_t)ArchInfo.Regs[rs1] - (int32_t)(ArchInfo.Regs[rs2]));
            break;
        case SLLW:
            ArchInfo.Regs[rd] = (int64_t)((uint32_t)ArchInfo.Regs[rs1] << (ArchInfo.Regs[rs2] & 0x1F));
            break;
        case SRLW:
            ArchInfo.Regs[rd] = (int64_t)((uint32_t)ArchInfo.Regs[rs1] >> (ArchInfo.Regs[rs2] & 0x1F));
            break;
        case SRAW:
            ArchInfo.Regs[rd] = (int64_t)((int32_t)ArchInfo.Regs[rs1] >> (ArchInfo.Regs[rs2] & 0x1F));
            break;
        // Reg-Imm
        case ADDI:
            ArchInfo.Regs[rd] = ArchInfo.Regs[rs1] + imm;
            break;
        case ANDI:
            ArchInfo.Regs[rd] = ArchInfo.Regs[rs1] & imm;
            break;
        case ORI:
            ArchInfo.Regs[rd] = ArchInfo.Regs[rs1] | imm;
            break;
        case XORI:
            ArchInfo.Regs[rd] = ArchInfo.Regs[rs1] ^ imm;
            break;
        case SLTI:
            ArchInfo.Regs[rd] = ((int64_t)ArchInfo.Regs[rs1] < (int64_t)imm);
            break;
        case SLTIU:
            ArchInfo.Regs[rd] = (ArchInfo.Regs[rs1] < imm);
            break;
        case SLLI:
            ArchInfo.Regs[rd] = ArchInfo.Regs[rs1] << (imm & 0x3F);
            break;
        case SRLI:
            ArchInfo.Regs[rd] = ArchInfo.Regs[rs1] >> (imm & 0x3F);
            break;
        case SRAI:
            ArchInfo.Regs[rd] = (int64_t)ArchInfo.Regs[rs1] >> (imm & 0x3F);
            break;
        case ADDIW:
            ArchInfo.Regs[rd] = (int64_t)((int32_t)ArchInfo.Regs[rs1] + (int32_t)imm);
            break;
        case SLLIW:
            ArchInfo.Regs[rd] = (int64_t)((uint32_t)ArchInfo.Regs[rs1] << (imm & 0x1F));
            break;
        case SRLIW:
            ArchInfo.Regs[rd] = (int64_t)((uint32_t)ArchInfo.Regs[rs1] >> (imm & 0x1F));
            break;
        case SRAIW:
            ArchInfo.Regs[rd] = (int64_t)((int32_t)ArchInfo.Regs[rs1] >> (imm & 0x1F));
            break;

        // Load
        case LB:
            raddr = ArchInfo.Regs[rs1] + imm;
            rdata0 = (uint64_t)MemRead(ArchInfo, raddr);
            ArchInfo.Regs[rd] = SignExt(rdata0, 8);
            break;
        case LBU:
            raddr = ArchInfo.Regs[rs1] + imm;
            rdata0 = (uint64_t)MemRead(ArchInfo, raddr);
            ArchInfo.Regs[rd] = rdata0;
            break;
        case LH:
            raddr = ArchInfo.Regs[rs1] + imm;
            rdata0 = (uint64_t)MemRead(ArchInfo, raddr);
            rdata1 = (uint64_t)MemRead(ArchInfo, raddr + 1) << 8;
            ArchInfo.Regs[rd] = SignExt(rdata0 | rdata1, 16);
            break;
        case LHU:
            raddr = ArchInfo.Regs[rs1] + imm;
            rdata0 = (uint64_t)MemRead(ArchInfo, raddr);
            rdata1 = (uint64_t)MemRead(ArchInfo, raddr + 1) << 8;
            ArchInfo.Regs[rd] = rdata0 | rdata1;
            break;
        case LW:
            raddr = ArchInfo.Regs[rs1] + imm;
            rdata0 = (uint64_t)MemRead(ArchInfo, raddr);
            rdata1 = (uint64_t)MemRead(ArchInfo, raddr + 1) << 8;
            rdata2 = (uint64_t)MemRead(ArchInfo, raddr + 2) << 16;
            rdata3 = (uint64_t)MemRead(ArchInfo, raddr + 3) << 24;
            ArchInfo.Regs[rd] = SignExt(rdata0 | rdata1 | rdata2 | rdata3, 32);
            break;
        case LWU:
            raddr = ArchInfo.Regs[rs1] + imm;
            rdata0 = (uint64_t)MemRead(ArchInfo, raddr);
            rdata1 = (uint64_t)MemRead(ArchInfo, raddr + 1) << 8;
            rdata2 = (uint64_t)MemRead(ArchInfo, raddr + 2) << 16;
            rdata3 = (uint64_t)MemRead(ArchInfo, raddr + 3) << 24;
            ArchInfo.Regs[rd] = rdata0 | rdata1 | rdata2 | rdata3;
            break;
        case LD:
            raddr = ArchInfo.Regs[rs1] + imm;
            rdata0 = (uint64_t)MemRead(ArchInfo, raddr);
            rdata1 = (uint64_t)MemRead(ArchInfo, raddr + 1) << 8;
            rdata2 = (uint64_t)MemRead(ArchInfo, raddr + 2) << 16;
            rdata3 = (uint64_t)MemRead(ArchInfo, raddr + 3) << 24;
            rdata4 = (uint64_t)MemRead(ArchInfo, raddr + 4) << 32;
            rdata5 = (uint64_t)MemRead(ArchInfo, raddr + 5) << 40;
            rdata6 = (uint64_t)MemRead(ArchInfo, raddr + 6) << 48;
            rdata7 = (uint64_t)MemRead(ArchInfo, raddr + 7) << 56;
            ArchInfo.Regs[rd] = rdata0 | rdata1 | rdata2 | rdata3 | rdata4 | rdata5 | rdata6 | rdata7;
            break;
        // Store
        case SB:
            waddr = ArchInfo.Regs[rs1] + imm;
            wdata0 = ArchInfo.Regs[rs2] & 0xFF;
            MemWrite(ArchInfo, waddr, wdata0);
            break;
        case SH:
            waddr = ArchInfo.Regs[rs1] + imm;
            wdata0 = ArchInfo.Regs[rs2] & 0xFF;
            wdata1 = (ArchInfo.Regs[rs2] >> 8) & 0xFF;
            MemWrite(ArchInfo, waddr, wdata0);
            MemWrite(ArchInfo, waddr + 1, wdata1);
            break;
        case SW:
            waddr = ArchInfo.Regs[rs1] + imm;
            wdata0 = ArchInfo.Regs[rs2] & 0xFF;
            wdata1 = (ArchInfo.Regs[rs2] >> 8) & 0xFF;
            wdata2 = (ArchInfo.Regs[rs2] >> 16) & 0xFF;
            wdata3 = (ArchInfo.Regs[rs2] >> 24) & 0xFF;
            MemWrite(ArchInfo, waddr, wdata0);
            MemWrite(ArchInfo, waddr + 1, wdata1);
            MemWrite(ArchInfo, waddr + 2, wdata2);
            MemWrite(ArchInfo, waddr + 3, wdata3);
            break;
        case SD:
            waddr = ArchInfo.Regs[rs1] + imm;
            wdata0 = ArchInfo.Regs[rs2] & 0xFF;
            wdata1 = (ArchInfo.Regs[rs2] >> 8) & 0xFF;
            wdata2 = (ArchInfo.Regs[rs2] >> 16) & 0xFF;
            wdata3 = (ArchInfo.Regs[rs2] >> 24) & 0xFF;
            wdata4 = (ArchInfo.Regs[rs2] >> 32) & 0xFF;
            wdata5 = (ArchInfo.Regs[rs2] >> 40) & 0xFF;
            wdata6 = (ArchInfo.Regs[rs2] >> 48) & 0xFF;
            wdata7 = (ArchInfo.Regs[rs2] >> 56) & 0xFF;
            MemWrite(ArchInfo, waddr, wdata0);
            MemWrite(ArchInfo, waddr + 1, wdata1);
            MemWrite(ArchInfo, waddr + 2, wdata2);
            MemWrite(ArchInfo, waddr + 3, wdata3);
            MemWrite(ArchInfo, waddr + 4, wdata4);
            MemWrite(ArchInfo, waddr + 5, wdata5);
            MemWrite(ArchInfo, waddr + 6, wdata6);
            MemWrite(ArchInfo, waddr + 7, wdata7);
            break;
        // Branch
        case BEQ:
            trigger = ArchInfo.Regs[rs1] == ArchInfo.Regs[rs2];
            if(trigger) { ArchInfo.PC += imm; taken = 1; }
            break;
        case BNE:
            trigger = ArchInfo.Regs[rs1] != ArchInfo.Regs[rs2];
            if(trigger) { ArchInfo.PC += imm; taken = 1; }
            break;
        case BLT:
            trigger = (int64_t)ArchInfo.Regs[rs1] < (int64_t)ArchInfo.Regs[rs2];
            if(trigger) { ArchInfo.PC += imm; taken = 1; }
            break;
        case BGE:
            trigger = (int64_t)ArchInfo.Regs[rs1] >= (int64_t)ArchInfo.Regs[rs2];
            if(trigger) { ArchInfo.PC += imm; taken = 1; }
            break;
        case BLTU:
            trigger = ArchInfo.Regs[rs1] < ArchInfo.Regs[rs2];
            if(trigger) { ArchInfo.PC += imm; taken = 1; }
            break;
        case BGEU:
            trigger = ArchInfo.Regs[rs1] >= ArchInfo.Regs[rs2];
            if(trigger) { ArchInfo.PC += imm; taken = 1; }
            break;
        // Jump
        case JAL:
            ArchInfo.Regs[rd] = ArchInfo.PC + 4;
            ArchInfo.PC += imm;
            taken = 1;
            break;
        case JALR:
            ArchInfo.Regs[rd] = ArchInfo.PC + 4;
            ArchInfo.PC = (ArchInfo.Regs[rs1] + imm) & ~1ULL;
            taken = 1;
            break;
        // other
        case LUI:
            ArchInfo.Regs[rd] = imm;
            break;
        case AUIPC:
            ArchInfo.Regs[rd] = ArchInfo.PC + imm;
            break;

        default:
            printf("Unknown or unimplemented instruction");
            assert(0);
    }

    ArchInfo.Mcycle++;
    ArchInfo.Regs[zero] = 0;
    if(taken == 0) ArchInfo.PC += 4;
    assert((ArchInfo.PC & 0x3UL) == 0);
    
    return ArchInfo;
}
